Skip to content


ai  101  pytorch  classification  nvidia  cuda  install  tensorrt  yolo  ardupilot  None  ros2  dds  micro ros  xrce  sitl  plugin  SITL  debug  rangefinder  pymavlink  mavros  gazebo  distance sensor  system_time  timesync  cmake  gtest  ctest  cpp  c++  format  fmt  multithreading  spdlog  camera  coordinate system  orb  matching  opencv  build  transformation  computer vision  homography  optical flow  of  trackers  cv  cyclonedds  eprosima  fastdds  simulation  config  ignition  bridge  sdf  tips  ign-transport  sensors  lidar  aptly  apt  encryption  pgp  docker  git  bundle  github  hooks  pre-commit  lxd  container  lxc  x11  profile  vscode  marpit  presentation  marp  markdown  mermaid  video  ffmpeg  gstreamer  cheat-sheet  sdp  v4l2loopback  gi  snippets  cheat Sheet  python  asyncio  future  click  cli  numpy  project  template  black  isort  docs  project document  docstrings  flake8  linter  git-hook  mypy  unittest  pytest  pylint  mock  iterator  generator  logging  tuple  namedtuple  typing  annotation  pyzmq  zmq  msgpack  action  namespace  remap  control2  ros2_control  gdb  qos  tag  plugins  msg  node  zero-copy  shm  tutorial  algorithm  calibration  diff  pid  dev  colcon  colcon_cd  rpi  arm  qemu  settings  behavior  plot  visualization  debugging  diagnostic  diagnostics  tutorials  gst  math  apm  rat_runtime_monitor  web  rosbridge  vue  binding  discovery  gazebo-classic  launch  spawn  cook  gps  imu  ray  gazebo_ros_ray_sensor  ultrsonic  range  ultrasonic  gazebo classic  wrench  effort  odom  ign  gz  xacro  ros_ign  diff_drive  odometry  joint_state  argument  OpaqueFunction  DeclareLaunchArgument  LaunchConfiguration  tmux  nav  slam  test  rclpy  executor  MultiThreadedExecutor  SingleThreadedExecutor  param  dynamic-reconfigure  service  client  setup.py  package.xml  parameter  parameters  custom  msgs  executers  pub  sub  rqt  rviz  rviz2  pose  marker  tf2  deb  package  setup  local_setup  rosdep  package manager  project settings  vcstool  cross-compiler  nano  texture  tmuxp  rootfs  embedded  zah  linux  rm  ubuntu  ip  ss  network  netstat  snap  deploy  ssh  systemd  mkdocs  extensions  socat  networking  serial  udp  tc  mtu  select  px4  robotics  kalman_filter  kalman  filter  control  todo  vscode-ext  json  yaml  schema  yocto  poky  world  gazebo_ros2_control  position_controller  effort_controller  velocity_controller  urdf  gazebo_ros_force  gazebo_ros_joint_state_publisher  robot_state_publisher  joint_state_publisher  projects  vrx  buoyancy 

Add rangefinder using SITL and gazebo#

Distance / Rangefinder has two type of message

- Distance sensor #132 (common)
- Rangefinder #173 (ardupilotmega) (reporting)

Distance sensor #132#

Distance sensor mavlink message - Distance sensor #132

  • Config SITL with RANGE FINDER
  • Send mavlink distance using pymavlink
params
RNGFND1_TYPE 10             # mavlink
RNGFND1_ORIENT 25           # down
RNGFND1_MAX_CM 1000         # cm
RNGFND1_MIN_CM 10           # cm
script
import time

from pymavlink import mavutil
from pymavlink.dialects.v20 import ardupilotmega

TO_MS = 1e3
UPDATE_RATE = 0.5
RNGFND_TYPE_MAVLINK = 10
SENSOR_ID = 1
SENSOR_MAX_CM = 1000
SENSOR_MIN_CM = 10
SENSOR_COVARIANCE = 0

SIM_CURRENT_READING_CM = 200

# Create the connection
master = mavutil.mavlink_connection("udp:127.0.0.1:14550")
# Wait a heartbeat before sending commands
master.wait_heartbeat()

t_start = time.time()
while True:
    time.sleep(UPDATE_RATE)
    boot_time = int((time.time() - t_start) * TO_MS)
    master.mav.distance_sensor_send(
        boot_time,
        SENSOR_MIN_CM,
        SENSOR_MAX_CM,
        SIM_CURRENT_READING_CM,
        ardupilotmega.MAV_DISTANCE_SENSOR_UNKNOWN,
        SENSOR_ID,
        ardupilotmega.MAV_SENSOR_ROTATION_PITCH_270,
        SENSOR_COVARIANCE,
    )
SITL/ sim_vehicle
./sim_vehicle.py -v ArduCopter \
-f quad -D \
--console \
--add-param-file /home/user/apm_ws/src/apm_bringup/config/range_finder.parm

params file

Add param file to sitl

--add-param-file <file path>


MAVROS#

Mavros has two plugins

  • distance_sensor (common msg #132)
  • rangefinder (ardupilot msg #173)

Demo#

  • Send distance using distance_sensor plugin (msg #132)
  • mavros open subscriber for each distance sensor declare as subscriber: true see config file example
  • Read data using rangefinder plugin (msg #173)
  • Echo distance data from fcu using cli (/mavros/rangfinder_pub)

publish distance data#

demo send random distance data
import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import Range

TOPIC_DISTANCE_READ = "/mavros/rangefinder_pub"
TOPIC_DISTANCE_WRITE = "/mavros/rangefinder_sub"
TOPIC_APM_RANGEFINDER = "/mavros/rangefinder/rangefinder"
DRONE_NO = 1
MIN_RANGE = 0.0
MAX_RANGE = 4.0
RANGE_SENSOR_TYPE = 1
SENSOR_ID = 1
COVARIANCE = 0

PUB_INTERVAL = 1/10

class RangeFinderNode(Node):
    def __init__(self):
        node_name = "range_finder"
        super().__init__(node_name)

        #mavros open subscriber, our node pub to it
        self.__range_pub = self.create_publisher(
            Range, TOPIC_DISTANCE_WRITE, qos_profile=qos_profile_sensor_data
        )
        self.create_subscription(
            Range, TOPIC_APM_RANGEFINDER, self.__apm_rangefinder_message_handler, qos_profile=qos_profile_sensor_data
        )

        self.create_timer(PUB_INTERVAL, self.__send_range_message)

    def __apm_rangefinder_message_handler(self, msg: Range):
        self.get_logger().info(f"apm rangefinder: {msg.range}")

    def __send_range_message(self, distance=2.0):
        sec, nanosec = self.get_clock().now().seconds_nanoseconds()
        range_msg = Range()
        range_msg.header.frame_id = "rangefinder"
        range_msg.header.stamp.sec = sec
        range_msg.header.stamp.nanosec = nanosec
        range_msg.range = float(distance)
        range_msg.radiation_type = RANGE_SENSOR_TYPE
        range_msg.min_range = MIN_RANGE
        range_msg.max_range = MAX_RANGE
        self.__range_pub.publish(range_msg)


def main(args=None):
    rclpy.init(args=args)
    node = RangeFinderNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        print("User exit")
    finally:
        node.destroy_node()
        rclpy.try_shutdown()


if __name__ == "__main__":
    main()

echo distance data#

ros2 topic echo /mavros/rangefinder_pub

config parma

config param expend settings as YAML

config multiple sensors#

/mavros/**/distance_sensor:
  ros__parameters:
    config: |
      rangefinder_pub:
        id: 0
        frame_id: "lidar"
        #orientation: PITCH_270 # sended by FCU
        field_of_view: 0.0  # XXX TODO
        send_tf: false
        sensor_position: {x:  0.0, y:  0.0, z:  -0.1}
      rangefinder_sub:
        subscriber: true
        id: 1
        orientation: PITCH_270
      rangefinder_fwd:
        subscriber: true
        id: 2
        orientation: PITCH_180

orientation

declare at /mavros/src/lib/enum_sensor_orientation.cpp


Demo to delete#

sitl
sim_vehicle.py -v ArduCopter -f gazebo-iris -A "--defaults /home/user/wasp_ws/src/wasp_bringup/config/copter.parm,/home/user/wasp_ws/src/wasp_bringup/config/gazebo-iris.parm" -I0 -m "--out=127.0.0.1:14552" -m "--load-module graph"